#include "state_troting.h"
#include "motor_control.h"
#include "global.h"
#include <stdio.h>
#include <math.h>

#define STANCE_RATIO 0.5
float bias[4]; // 相位差 trot步态对角相差0.5

/* 相位计算 */
static inline void calc_phase(uint8_t id, float *contact_ptr, float *phase_ptr)
{
    float period = global.period;

    /* 计算时间偏差 */
    float normalT = fmod(global.run_time + period * bias[id], period) / period;
    if (normalT < STANCE_RATIO) // 站立态
    {
        *contact_ptr = 1;
        *phase_ptr = normalT / STANCE_RATIO;
    }
    else // 摆动态
    {
        *contact_ptr = 0;
        *phase_ptr = (normalT - STANCE_RATIO) / (1.0 - STANCE_RATIO);
    }
}

/* 根据当前相位计算摆线轨迹 */
static inline void calc_cycloid(float *x_ptr, float *z_ptr, float contact, float phase)
{
    float p, x, z;
    float height = global.center_height;

    /* 计算复合摆线x, z */
    p = M_2PI * phase;
    if (contact) /* 足端触地时 */
    {
        x = global.step_length * (sinf(p) / M_2PI - phase + 0.5f);
        if (phase < 0.5f)
        {
            z = -2 * global.footstamp_height * (phase - sinf(2 * p) / M_4PI) - height;
        }
        else
        {
            z = -2 * global.footstamp_height * (1 - phase + sinf(2 * p) / (M_4PI)) - height;
        }
    }
    else /* 足端摆动 */
    {
        x = global.step_length * (phase - sinf(p) / (M_2PI)-0.5f);

        if (phase < 0.5f)
        {
            z = 2 * global.footlift_height * (phase - sinf(2 * p) / M_4PI) - height;
        }
        else
        {
            z = 2 * global.footlift_height * (1 - phase + sinf(2 * p) / (M_4PI)) - height;
        }
    }
    *x_ptr = x;
    *z_ptr = z;
}

/* 运动学逆解 */
static inline void kinematic_inverse_solution(float *sita1, float *sita2, float x, float z)
{
    /* 足端偏移的虚拟腿转角 */
    float phi = atan2f(-z, x);
    if (phi > MAX_A)
    {
        phi = MAX_A;
        printf("larger than the maximum angle\n");
    }
    else if (phi < MIN_A)
    {
        phi = MIN_A;
        printf("less than the minimum angle\n");
    }

    /* 虚拟腿长 */
    float OD = sqrtf(z * z + x * x);
    if (OD > MAX_L)
    {
        OD = MAX_L;
        printf("Longer than the longest leg\n");
    }
    else if (OD < MIN_L)
    {
        OD = MIN_L;
        printf("Shorter than the shortest leg\n");
    }

    float OBD = acosf((L1 * L1 + L3 * L3 - OD * OD) / (2 * L1 * L3));
    float OC = sqrtf(L1 * L1 + L2 * L2 - 2 * L1 * L2 * cosf(OBD));
    float BOC = acosf((L1 * L1 + OC * OC - L2 * L2) / (2 * L1 * OC));
    float COD = acosf((OC * OC + OD * OD - (L3 - L2) * (L3 - L2)) / (2 * OC * OD));

    float alpha = phi - COD - BOC;
    float beta = phi - COD + BOC;

    *sita1 = alpha;
    *sita2 = M_PI - beta;
}

#define KP 14.0f
#define KD 0.15f

void troting_enter(void)
{
    printf("troting enter\n");

    param.kp = KP;
    param.kd = KD;
    param.torque = 0;
    param.velocity = 0;
    param.position = 0;

    /* 设置相位差 */
    bias[0] = 0.5;
    bias[1] = 0.0;
    bias[2] = 0.5;
    bias[3] = 0.0;

    /* 数据清零并开启定时器 */
    __HAL_TIM_SetCounter(&GLOBAL_TIM, 0);
    HAL_TIM_Base_Start(&GLOBAL_TIM);
}

void troting_run(void)
{
    float contact, phase, x, z, sita1, sita2;

    for (size_t i = 0; i < 4; i++)
    {
        /* 计算相位 */
        calc_phase(i, &contact, &phase);
        /* 计算摆线 */
        calc_cycloid(&x, &z, contact, phase);
        /* 运动学逆解 */
        kinematic_inverse_solution(&sita1, &sita2, x, z);
        /* 发送电机参数 */
        param.position = sita1;
        motor_set_param(i * 2, &param);
        param.position = sita2;
        motor_set_param(i * 2 + 1, &param);
    }
}

void troting_exit(void)
{
    printf("troting exit\n");
}
